
#include <stdbool.h>
#include <stdint.h>
#include "algorithm.h"

#define RAW_WINDOW_SIZE          4
#define RAW_WINDOW_SIZE_MASK     (RAW_WINDOW_SIZE - 1)

typedef struct
{
    algo_axis_data_t raw[RAW_WINDOW_SIZE];
    algo_axis_data_t average;
    algo_axis_data_t filtered;
    uint8_t idx;
    bool first;
}windows_t;

static windows_t windows;

static void algo_filter_init(void)
{
    memset( &windows, 0, sizeof(windows) );
    windows.first = true; 
}

void algo_filter_with_window( algo_axis_data_t* p_axis_data)
{
    if( windows.first )
    {
        windows.first = false;
        for(int i = 0; i < RAW_WINDOW_SIZE; i ++)
        {
            windows.raw[i].x = p_axis_data->x;
            windows.raw[i].y = p_axis_data->y;
            windows.raw[i].z = p_axis_data->z;
        }
        windows.average.x = p_axis_data->x * RAW_WINDOW_SIZE;
        windows.average.y = p_axis_data->y * RAW_WINDOW_SIZE;
        windows.average.z = p_axis_data->z * RAW_WINDOW_SIZE; 
    }
    
    windows.average.x = (short)windows.average.x - windows.raw[windows.idx].x + p_axis_data->x;
    windows.average.y = (short)windows.average.y - windows.raw[windows.idx].y + p_axis_data->y;
    windows.average.z = (short)windows.average.z - windows.raw[windows.idx].z + p_axis_data->z;

    windows.filtered.x = windows.average.x / RAW_WINDOW_SIZE;
    windows.filtered.y = windows.average.y / RAW_WINDOW_SIZE;
    windows.filtered.z = windows.average.z / RAW_WINDOW_SIZE;

    windows.raw[windows.idx].x = p_axis_data->x;
    windows.raw[windows.idx].y = p_axis_data->y;
    windows.raw[windows.idx].z = p_axis_data->z;

    windows.idx ++;
    if( windows.idx >= RAW_WINDOW_SIZE )
    {
        windows.idx = 0;
    }
}
